/*
 *
 * Author: Andreas Wüstenberg (andreas.wuestenberg@rwth-aachen.de), Clemens Boennen (clemens.boennen@rwth-aachen.de)
 *
 */

#include "Sender.h"

#include "rtps/rtps.h"
#include "rtps/entities/Domain.h"
#include "HelloWorld.h"

#include "ucdr/microcdr.h"
#include "rtps/messages/MessageTypes.h"


Sender_Init(struct Sender *pSender, uint32_t numSamples){
    pSender->m_numSamples = numSamples;
    pSender->m_index = 1;
    Sender_prepareRTPS(pSender);
}

void Sender_prepareRTPS(struct Sender *pSender){
    rtps_Domain_Construct(&pSender->m_domain);

    rtps_Participant *part = rtps_Domain_createParticipant(&pSender->m_domain);
    if(part == NULL){
        printf("Failed to create participant\n");
        return;
    }
    rtps_Domain_completeInit(&pSender->m_domain);
    pSender->mp_dataWriter = rtps_Domain_createWriter(&pSender->m_domain, part, "HelloWorldTopic", 
                                                    "HelloWorld", true, false);

    if(pSender->mp_dataWriter == NULL){
        printf("Failed to create endpoints.\n");
        return;
    }
}

void Sender_run(struct Sender *pSender) {
    printf("Waiting 5 sec for startup....\n");
    sys_msleep(5000); // Wait for initialization
    printf("Go!\n");

    printf("Sending HelloWorldPackets: \n", pSender->m_numSamples);

    for (uint32_t i = 0; i < pSender->m_numSamples; i++) {
        Sender_runWithIndex(pSender, pSender->m_index);
        printf("Hello WorldPacket sent: Index %d \n", pSender->m_index);
        pSender->m_index++;
        sys_msleep(100);
    }
}

void Sender_runWithIndex(struct Sender *pSender, uint32_t index){
    struct HelloWorld hw;

    hw.index = index;
    strcpy(hw.message, "HelloWorld");

    uint32_t tsize = HelloWorld_size_of_topic(&hw, 0) + 4;

    uint8_t buffer[tsize];

    ucdrBuffer writer;
    ucdr_init_buffer(&writer, buffer, tsize);

    const uint16_t zero_options = 0;
    ucdr_serialize_array_uint8_t(&writer, SCHEME_CDR_LE, sizeof(SCHEME_CDR_LE));
    ucdr_serialize_uint16_t(&writer, zero_options);

    HelloWorld_serialize_topic(&writer, &hw);
    struct rtps_CacheChange *change;
    if(pSender->mp_dataWriter->m_bstateful)
        change = rtps_StatefulWriter_newChange((struct rtps_StatefulWriter *)pSender->mp_dataWriter, ALIVE, buffer, tsize);
    else
        change = rtps_StatelessWriter_newChange((struct rtps_StatelessWriter *)pSender->mp_dataWriter, ALIVE, buffer, tsize);
    
    if(change == NULL){
        printf("History full. Abort. \n");
        return;
    }
}

